#include "motion.h"
#include "algorithm.h"
#include "motor.h"
#include "oled_hardware_i2c.h"
#include "stdio.h"
#include "stdlib.h"
#include "uart.h"

#define TAG "Motion"

#define L 16.0f
///////////////////////////Helper Function///////////////////////////
///////////////////////////Helper Function///////////////////////////
void limit_value(int16_t *left_speed, int16_t *right_speed, int16_t max_speed)
{
  // 确保最大速度为正值
  int16_t abs_max = abs(max_speed);
  if (abs_max == 0)
    return; // 避免除以零

  // 保存原始速度的符号
  int16_t left_sign = (*left_speed >= 0) ? 1 : -1;
  int16_t right_sign = (*right_speed >= 0) ? 1 : -1;

  // 计算当前速度的绝对值
  int16_t left_abs = abs(*left_speed);
  int16_t right_abs = abs(*right_speed);

  // 找到当前最大速度的绝对值
  int16_t current_max = (left_abs > right_abs) ? left_abs : right_abs;

  // 如果当前速度未超过限制，则不需要调整
  if (current_max <= abs_max)
  {
    return;
  }

  // 计算缩放比例
  float scale_factor = (float)abs_max / current_max;

  // 按比例缩放速度，使用四舍五入提高精度，保持方向不变
  if (left_abs > 0)
  {
    *left_speed = (int16_t)round(left_sign * left_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else
  {
    *left_speed = 0;
  }

  if (right_abs > 0)
  {
    *right_speed = (int16_t)round(right_sign * right_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else
  {
    *right_speed = 0;
  }
}
void motion_torque_ctrl(int16_t left_torque, int16_t right_torque)
{
#define SELECT_DIR(torque) \
  (torque > 0 ? MOTOR_FORWARD : (torque < 0 ? MOTOR_BACKWARD : MOTOR_NEUTRAL))
  motor_set_mode(MOTOR_REAR | MOTOR_LEFT, SELECT_DIR(left_torque));
  motor_set_mode(MOTOR_REAR | MOTOR_RIGHT, SELECT_DIR(right_torque));

  limit_value(&left_torque, &right_torque, 1000);

  motor_set_torque(MOTOR_REAR | MOTOR_LEFT, my_abs(left_torque));
  motor_set_torque(MOTOR_REAR | MOTOR_RIGHT, my_abs(right_torque));
}

//////////////////////////End of Helper Function///////////////////////////

///////////////////////Algorithm///////////////////////
///////////////////////Algorithm///////////////////////

//////////////////////MOVE SECTION/////////////////////

static int16_t target_left_speed = 0, target_right_speed = 0,speed=0;
static pid_control_t rl_speed_ctrler = {0}, rr_speed_ctrler = {0};

void motion_move_pool(float delata_seconds)
{
  int16_t left_error =
      target_left_speed - (int16_t)motor_get_speed(MOTOR_REAR | MOTOR_LEFT);
  int16_t right_error =
      target_right_speed - (int16_t)motor_get_speed(MOTOR_REAR | MOTOR_RIGHT);

  int16_t adjustment_l =
      pid_update(&rl_speed_ctrler, left_error, delata_seconds);
  int16_t adjustment_r =
      pid_update(&rr_speed_ctrler, right_error, delata_seconds);

  motion_torque_ctrl(target_left_speed + adjustment_l,
                     target_right_speed + adjustment_r);
}
void motion_set_speed(int16_t _speed)
{
  target_left_speed = _speed;
  target_right_speed = _speed;
}
//////////////////////TURN SECTION/////////////////////
void motion_turn_pool(float err, float k)
{
  static pid_control_t speed_ctrler = {0};
  //need be set by other function just for complie
  float k_adjust;
  float radius_adjustment = pid_update(&speed_ctrler, err, -1);
  speed=_speed;
  k_adjust = speed * k;
  // my_printf("left speed =%d right speed =%d\n", current_ls, current_rs);

  target_left_speed = speed - (int16_t)radius_adjustment * k_adjust;
  target_right_speed = speed + (int16_t)radius_adjustment * k_adjust;

  // if (my_abs(radius_adjustment) < 1e-6)
  //   my_printf("L:%d R:%d adjust:%.1f and ki=%f\n", current_ls, current_rs,
  //             radius_adjustment, hh.Ki);

  motion_speed_ctrl(current_ls, current_rs);
}

///////////////////////END OF ALGORITHM///////////////////////
///////////////////////END OF ALGORITHM///////////////////////

void motion_init(void)
{
  pid_init(&rl_speed_ctrler, 2, 0, 0, 500);
  pid_init(&rr_speed_ctrler, 2, 0, 0, 500);

  motor_init();
}